"""
--------------------------------------------------------------------------------
功能:PS2手柄控制机械臂程序
日期:2025-06-06
作者:Unirobot
QQ交流群:811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司
--------------------------------------------------------------------------------
"""

import time
import queue
import threading
from src.joy import joy_module
from src.arm_control import manager
from src.arm_control import custom_font

#=======================================加载自定义字体
time.sleep(1)
font = custom_font.CustomFont()
print(font.color_text(font.compony_msg, color="cyan", bold=True))
print(font.color_text(font.font_load_success, color="green", bold=True))

#=======================================创建机械臂实例,以及机械臂相关参数
arm = manager.RobotManager()
arm.arm_init_action()#机械臂执行初始化动作
print(font.color_text(font.arm_load_success, color="green", bold=True))
arm_x_step = 5
arm_y_step = 5
arm_z_step = 5
joystick_name = ''
joystick_data = 0
joystick_action_move = False
joystick_current_numbers = 0
#=======================================创建线程
arm_command_queue = queue.Queue(maxsize=1)  # 限制队列大小防止积压
def arm_worker():
    """机械臂控制线程"""
    while True:
        command = arm_command_queue.get()  # 获取包含两个参数的元组
        try:
            arm.save_ang[0], arm.save_ang[1], arm.save_ang[2] = command
            arm.arm_move(arm.save_ang[0], arm.save_ang[1], arm.save_ang[2], 1)
        except Exception as e:
            print(f"[ARM ERROR] 机械臂控制错误: {e}")
# 启动机械臂控制线程
arm_thread = threading.Thread(target=arm_worker, daemon=True)
arm_thread.start()

#=======================================创建监听器实例
gamepad = joy_module.GamepadListener(device_path='/dev/input/js0')
gamepad.start() #启动监听
print(font.color_text(font.ps2_load_success, color="green", bold=True))
AXIS_THRESHOLD = 30000  # 摇杆阈值 - 小于此值视为未移动
# 摇杆状态字典 - 跟踪摇杆的模拟值
axis_states = {'Left Stick X', 'Left Stick Y', 'Right Stick X', 'Right Stick Y'}
# 按键状态字典. 跟踪所有按键的当前状态
button_states = {
    'A': False,'B': False,'X': False,'Y': False, #带有字母的按钮
    'Start': False,'Select': False,'Mode/Xbox': False, #中间三个小按钮
    'Left': False,'Right': False,'Up': False,'Down': False, #左侧上，下，左，右四个按键
    'Left Shoulder': False,'Right Shoulder': False,'L2': False, 'R2': False} #L1,L2,R1,R2按键
# 主程序循环
try:
    print(font.color_text(font.ps2_arm_commond, color="cyan", bold=True))
    while True:
        # 获取所有未处理的事件
        events = gamepad.return_status(clear_queue=True)
        
        # 更新按键状态
        for event in events:
            if event['type'] == 'button':
                button_name = event['name']
                # 更新按钮状态
                button_states[button_name] = bool(event['value'])
                
            elif event['type'] == 'axis':
                # 处理方向键
                if event['name'] == 'D-pad X':
                    button_states['Left'] = (event['value'] == -32767)
                    button_states['Right'] = (event['value'] == 32767)
                    
                elif event['name'] == 'D-pad Y':
                    button_states['Up'] = (event['value'] == -32767)
                    button_states['Down'] = (event['value'] == 32767)
                
                # 处理L2/R2触发键
                elif event['name'] == 'LT':
                    button_states['L2'] = (event['value'] == 32767)
                    
                elif event['name'] == 'RT':
                    button_states['R2'] = (event['value'] == 32767)
                    print(button_states['R2'], event['value'])

                #获取摇杆状态
                elif event['name'] in axis_states:
                    joystick_name = event['name']
                    joystick_data = event['value']
                    if joystick_data != 0:
                        joystick_action_move = True
                    else:
                        joystick_action_move = False
                        joystick_current_numbers = 0
        
        #将摇杆值映射到机械臂控制量
        if joystick_action_move == True:
            joystick_current_numbers += 1
            if joystick_name == 'Left Stick X':
                if joystick_data <= -AXIS_THRESHOLD:
                    arm.save_ang[1] -= arm_y_step
                if joystick_data >= AXIS_THRESHOLD:
                    arm.save_ang[1] += arm_y_step
            elif joystick_name == 'Left Stick Y':
                if joystick_data <= -AXIS_THRESHOLD:
                    arm.save_ang[0] += arm_x_step
                if joystick_data >= AXIS_THRESHOLD:
                    arm.save_ang[0] -= arm_x_step
            elif joystick_name == 'Right Stick Y':
                if joystick_data <= -AXIS_THRESHOLD:
                    arm.save_ang[2] += arm_z_step
                if joystick_data >= AXIS_THRESHOLD:
                    arm.save_ang[2] -= arm_z_step
            
        
        # 检查所有按键状态，执行持续动作
        if button_states['A']:               #持续按下A
            pass
        if button_states['B']:               #持续按下B
            pass
        if button_states['X']:               #持续按下X
            pass
        if button_states['Y']:               #持续按下Y
            pass
        if button_states['Start']:           #持续按下Start
            pass
        if button_states['Select']:          #持续按下Select
            pass
        if button_states['Mode/Xbox']:       #持续按下Mode
            pass
        if button_states['Left']:            #持续按下Left
            pass
        if button_states['Right']:           #持续按下Right
            pass
        if button_states['Up']:              #持续按下Up
            pass
        if button_states['Down']:            #持续按下Down
            pass
        if button_states['L2']:              #持续按下L2
            pass
        if button_states['R2']:              #持续按下R2
            pass
        if button_states['Left Shoulder']:   #持续按下L1
            # print(font.color_text('爪子打开', color="green", bold=True))
            arm.catch_sth()
        if button_states['Right Shoulder']:  #持续按下R1
            # print(font.color_text('爪子闭合', color="green", bold=True))
            arm.release_sth()

        #机械臂运动学限位
        arm.save_ang[0], arm.save_ang[1], arm.save_ang[2] = arm.three_axis_data_limit(arm.save_ang[0], arm.save_ang[1], arm.save_ang[2])
        if arm_command_queue.empty():
            # 添加位移数据到队列
            arm_command_queue.put_nowait((
                arm.save_ang[0], arm.save_ang[1], arm.save_ang[2]
            ))
        # 控制循环速度
        time.sleep(0.0001)
        
except KeyboardInterrupt:
    print("程序退出")

finally:
    # 确保停止监听器
    gamepad.stop()
